/*
 * Copyright (C) 2014
 *
 * Brick Yang <printfxxx@163.com>
 *
 * This program is free software. You can redistribute it and/or
 * modify it as you like.
 */

/**
 * @file	mdio_tool.c
 * @brief	Tool for R/W MDIO/MII registers
 */

#include <stdio.h>
#include <errno.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <inttypes.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <linux/mdio.h>
#include <linux/sockios.h>

#define LINE_REGS	8

struct mdio_op {
	const char *name;
	int (*handler)(int argc, const char *argv[]);
	const char *help;
};

static inline bool mdio_phy_id_is_c45(uint16_t phy_id)
{
	return !!(phy_id & MDIO_PHY_ID_C45);
}

static inline uint16_t mdio_phy_id_prtad(uint16_t phy_id)
{
	return (phy_id & MDIO_PHY_ID_PRTAD) >> 5;
}

static inline uint16_t mdio_phy_id_devad(uint16_t phy_id)
{
	return phy_id & MDIO_PHY_ID_DEVAD;
}

static inline uint32_t mdio_c45_addr(uint16_t devad, uint16_t regnum)
{
#define MDIO_ADDR_C45		(1 << 30)
#define MDIO_DEVAD_C45_SHIFT	16
	return MDIO_ADDR_C45 | devad << MDIO_DEVAD_C45_SHIFT | regnum;
}

static void parse_phy_id(const char *str, uint16_t *phy_id)
{
	uint16_t prtad, devad;
	const char *colon;

	colon = strchr(str, ':');
	if (colon) {
		prtad = strtoul(str, NULL, 16) & (MDIO_PHY_ID_PRTAD >> 5);
		devad = strtoul(colon + 1, NULL, 16) & MDIO_PHY_ID_DEVAD;
		*phy_id = mdio_phy_id_c45(prtad, devad);
	} else {
		*phy_id = strtoul(str, NULL, 16) & MDIO_PHY_ID_DEVAD;
	}
}

static int mdio_query(int argc, const char *argv[])
{
	int rc, sock = -1;
	struct ifreq ifr = {};
	struct mii_ioctl_data *mii;

	switch (argc) {
	case 2:
		strncpy(ifr.ifr_name, argv[1], sizeof(ifr.ifr_name) - 1);
		mii = (struct mii_ioctl_data *)(&ifr.ifr_data);
		break;
	default:
		fprintf(stderr, "ERROR: Bad format for command \"%s\"\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	if ((sock = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
		fprintf(stderr, "ERROR: failed to create socket\n");
		rc = -ENODEV;
		goto err;
	}

	if (ioctl(sock, SIOCGMIIPHY, &ifr) == -1) {
		fprintf(stderr,"ERROR: failed to get address of PHY\n");
		rc = -EIO;
		goto err;
	}
	printf("%04" PRIx16 "\n", mii->phy_id);

	close(sock);
	return 0;
err:
	if (sock >= 0) {
		close(sock);
	}
	return rc;
}

static int mdio_read(int argc, const char *argv[])
{
	int rc, sock = -1;
	uint16_t reg_base;
	unsigned int i, n, skip, count = 1;
	struct ifreq ifr = {};
	struct mii_ioctl_data *mii;

	switch (argc) {
	case 5:
		count = strtoul(argv[4], NULL, 16);
	case 4:
		strncpy(ifr.ifr_name, argv[1], sizeof(ifr.ifr_name) - 1);
		mii = (struct mii_ioctl_data *)(&ifr.ifr_data);
		parse_phy_id(argv[2], &mii->phy_id);
		mii->reg_num = strtoul(argv[3], NULL, 16) & 0xffff;
		break;
	default:
		fprintf(stderr, "ERROR: Bad format for command \"%s\"\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	if ((sock = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
		fprintf(stderr, "ERROR: failed to create socket\n");
		rc = -ENODEV;
		goto err;
	}

	skip = (mii->reg_num % LINE_REGS);
	reg_base = mii->reg_num - skip;

	if (argc == 4) {
		if (ioctl(sock, SIOCGMIIREG, &ifr) == -1) {
			fprintf(stderr,"ERROR: failed to read mii register\n");
			rc = -EIO;
			goto err;
		}
		printf("%04" PRIx16 "\n", mii->val_out);
	} else while (count) {
		n = LINE_REGS - skip;
		n = n < count ? n : count;
		printf("%04" PRIx16 "%-*c", reg_base, skip * 5 + 1, ':');
		for (i = 0; i < n; i++) {
			if (ioctl(sock, SIOCGMIIREG, &ifr) == -1) {
				fprintf(stderr,"ERROR: failed to read mii register\n");
				rc = -EIO;
				goto err;
			}
			printf(" %04" PRIx16, mii->val_out);
			mii->reg_num++;
		}
		reg_base += LINE_REGS;
		count -= n;
		printf("\n");
		skip = 0;
	}

	close(sock);
	return 0;
err:
	if (sock >= 0) {
		close(sock);
	}
	return rc;
}

static int mdio_write(int argc, const char *argv[])
{
	int rc, sock = -1;
	struct ifreq ifr = {};
	struct mii_ioctl_data *mii;

	switch (argc) {
	case 5:
		strncpy(ifr.ifr_name, argv[1], sizeof(ifr.ifr_name) - 1);
		mii = (struct mii_ioctl_data *)(&ifr.ifr_data);
		parse_phy_id(argv[2], &mii->phy_id);
		mii->reg_num = strtoul(argv[3], NULL, 16) & 0xffff;
		mii->val_in = strtoul(argv[4], NULL, 16) & 0xffff;
		break;
	default:
		fprintf(stderr, "ERROR: Bad format for command \"%s\"\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	if ((sock = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
		fprintf(stderr, "ERROR: failed to create socket\n");
		rc = -ENODEV;
		goto err;
	}

	if (ioctl(sock, SIOCSMIIREG, &ifr) == -1) {
		fprintf(stderr,"ERROR: failed to write mii register\n");
		rc = -EIO;
		goto err;
	}

	close(sock);
	return 0;
err:
	if (sock >= 0) {
		close(sock);
	}
	return rc;
}

static int mdio_kread(int argc, const char *argv[])
{
	int rc;
	char str[64];
	bool is_c45;
	FILE *fp_bus = NULL, *fp_addr = NULL,
	     *fp_regnum = NULL, *fp_regval = NULL;
	uint16_t prtad, devad;
	uint16_t reg_base;
	unsigned int i, n, skip, count = 1;
	struct mii_ioctl_data mii;

	switch (argc) {
	case 5:
		count = strtoul(argv[4], NULL, 16);
	case 4:
		parse_phy_id(argv[2], &mii.phy_id);
		mii.reg_num = strtoul(argv[3], NULL, 16) & 0xffff;
		break;
	default:
		fprintf(stderr, "ERROR: Bad format for command \"%s\"\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	if (!(fp_bus = fopen("/proc/mdio_tool/bus", "w")) || setvbuf(fp_bus, NULL, _IONBF, 0)
	||  !(fp_addr = fopen("/proc/mdio_tool/addr", "w")) || setvbuf(fp_addr, NULL, _IONBF, 0)
	||  !(fp_regnum = fopen("/proc/mdio_tool/regnum", "w")) || setvbuf(fp_regnum, NULL, _IONBF, 0)
	||  !(fp_regval = fopen("/proc/mdio_tool/regval", "r")) || setvbuf(fp_regval, NULL, _IONBF, 0)) {
		fprintf(stderr,"ERROR: failed to open files in /proc/mdio_tool/\n");
		rc = -ENODEV;
		goto err;
	}

	if (mdio_phy_id_is_c45(mii.phy_id)) {
		is_c45 = true;
		prtad = mdio_phy_id_prtad(mii.phy_id);
		devad = mdio_phy_id_devad(mii.phy_id);
	} else {
		is_c45 = false;
		prtad = mii.phy_id;
		devad = 0;
	}

	if (fputs(argv[1], fp_bus) == EOF || fprintf(fp_addr, "%u", prtad) < 0) {
		fprintf(stderr,"ERROR: failed to set current mdio bus\n");
		rc = -ENODEV;
		goto err;
	}

	skip = (mii.reg_num % LINE_REGS);
	reg_base = mii.reg_num - skip;

	if (argc == 4) {
		if (fprintf(fp_regnum, "0x%x", is_c45 ? mdio_c45_addr(devad, mii.reg_num) : mii.reg_num) < 0
		||  !fgets(str, sizeof(str), fp_regval)) {
			fprintf(stderr,"ERROR: failed to read mii register\n");
			rc = -EIO;
			goto err;
		}
		mii.val_out = strtoul(str, NULL, 0);
		printf("%04" PRIx16 "\n", mii.val_out);
	} else while (count) {
		n = LINE_REGS - skip;
		n = n < count ? n : count;
		printf("%04" PRIx16 "%-*c", reg_base, skip * 5 + 1, ':');
		for (i = 0; i < n; i++) {
			rewind(fp_regnum);
			rewind(fp_regval);
			if (fprintf(fp_regnum, "0x%x", is_c45 ? mdio_c45_addr(devad, mii.reg_num) : mii.reg_num) < 0
			||  !fgets(str, sizeof(str), fp_regval)) {
				fprintf(stderr,"ERROR: failed to read mii register\n");
				rc = -EIO;
				goto err;
			}
			mii.val_out = strtoul(str, NULL, 0);
			printf(" %04" PRIx16, mii.val_out);
			mii.reg_num++;
		}
		reg_base += LINE_REGS;
		count -= n;
		printf("\n");
		skip = 0;
	}

	fclose(fp_bus);
	fclose(fp_addr);
	fclose(fp_regnum);
	fclose(fp_regval);
	return 0;
err:
	if (fp_regval) {
		fclose(fp_regval);
	}
	if (fp_regnum) {
		fclose(fp_regnum);
	}
	if (fp_addr) {
		fclose(fp_addr);
	}
	if (fp_bus) {
		fclose(fp_bus);
	}
	return rc;
}

static int mdio_kwrite(int argc, const char *argv[])
{
	int rc;
	bool is_c45;
	FILE *fp_bus = NULL, *fp_addr = NULL,
	     *fp_regnum = NULL, *fp_regval = NULL;
	uint16_t prtad, devad;
	struct mii_ioctl_data mii;

	switch (argc) {
	case 5:
		parse_phy_id(argv[2], &mii.phy_id);
		mii.reg_num = strtoul(argv[3], NULL, 16) & 0xffff;
		mii.val_in = strtoul(argv[4], NULL, 16) & 0xffff;
		break;
	default:
		fprintf(stderr, "ERROR: Bad format for command \"%s\"\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	if (!(fp_bus = fopen("/proc/mdio_tool/bus", "w")) || setvbuf(fp_bus, NULL, _IONBF, 0)
	||  !(fp_addr = fopen("/proc/mdio_tool/addr", "w")) || setvbuf(fp_addr, NULL, _IONBF, 0)
	||  !(fp_regnum = fopen("/proc/mdio_tool/regnum", "w")) || setvbuf(fp_regnum, NULL, _IONBF, 0)
	||  !(fp_regval = fopen("/proc/mdio_tool/regval", "w")) || setvbuf(fp_regval, NULL, _IONBF, 0)) {
		fprintf(stderr,"ERROR: failed to open files in /proc/mdio_tool/\n");
		rc = -ENODEV;
		goto err;
	}

	if (mdio_phy_id_is_c45(mii.phy_id)) {
		is_c45 = true;
		prtad = mdio_phy_id_prtad(mii.phy_id);
		devad = mdio_phy_id_devad(mii.phy_id);
	} else {
		is_c45 = false;
		prtad = mii.phy_id;
		devad = 0;
	}

	if (fputs(argv[1], fp_bus) == EOF || fprintf(fp_addr, "%u", prtad) < 0) {
		fprintf(stderr,"ERROR: failed to set current mdio bus\n");
		rc = -ENODEV;
		goto err;
	}

	if (fprintf(fp_regnum, "0x%x", is_c45 ? mdio_c45_addr(devad, mii.reg_num) : mii.reg_num) < 0
	||  fprintf(fp_regval, "0x%x", mii.val_in) < 0) {
		fprintf(stderr,"ERROR: failed to write mii register\n");
		rc = -EIO;
		goto err;
	}

	fclose(fp_bus);
	fclose(fp_addr);
	fclose(fp_regnum);
	fclose(fp_regval);
	return 0;
err:
	if (fp_regval) {
		fclose(fp_regval);
	}
	if (fp_regnum) {
		fclose(fp_regnum);
	}
	if (fp_addr) {
		fclose(fp_addr);
	}
	if (fp_bus) {
		fclose(fp_bus);
	}
	return rc;
}

const struct mdio_op mdio_op_handler[] = {
	{"q",  mdio_query,  "[ dev ]                                           - Query address of PHY"},
	{"r",  mdio_read,   "[ dev ] [[ prtad: ]][ devad ] [ reg ] [[ count ]] - Read mdio register"},
	{"w",  mdio_write,  "[ dev ] [[ prtad: ]][ devad ] [ reg ] [ data ]    - Write mdio register"},
	{"kr", mdio_kread,  "[ bus ] [[ prtad: ]][ devad ] [ reg ] [[ count ]] - Read mdio register via kmod"},
	{"kw", mdio_kwrite, "[ bus ] [[ prtad: ]][ devad ] [ reg ] [ data ]    - Write mdio register via kmod"},
	{NULL, NULL, NULL}
};

static void print_help(void)
{
	const struct mdio_op *op = mdio_op_handler;

	printf("Usage:\n");
	printf("mdio_tool [q|r|w]\n");
	printf("\n");
	printf("type: q|r|w - query|read|write\n");
	printf("\n");
	while (op->name != NULL) {
		if (op->help != NULL) {
			printf("mdio_tool %-1s %s\n", op->name, op->help);
		}
		op++;
	}
}

int main(int argc, const char *argv[])
{
	int rc;
	const struct mdio_op *op = mdio_op_handler;

	if (argc < 2) {
		fprintf(stderr, "ERROR: too few argument!\n");
		rc = -EINVAL;
		goto err;
	}

	if (strcmp("-h", argv[1]) == 0) {
		print_help();
		goto ok;
	}

	while (op->name != NULL) {
		if (strcmp(op->name, argv[1]) == 0) {
			if (!(rc = op->handler(argc - 1, argv + 1))) {
				goto ok;
			} else {
				goto err;
			}
		}
		op++;
	}

	fprintf(stderr, "ERROR: unknown command \"%s\"\n", argv[1]);
	rc = -EINVAL;
err:
	if (rc == -EINVAL) {
		print_help();
	}
	errno = -rc;
	return -1;
ok:
	return 0;
}
